#include "main.h"
#include "usart.h"
#include "UPRE_GYRO.h"

struct Gyro gyro = {


        .State=0,
        .Angle_pitch=0,					//俯仰角
        .Angle_roll=0,					//横滚角
        .Angle_azimuth=0,				//方位角
        .W_x=0,		       			  //惯导角速率
        .W_y=0,
        .A_y=0,
        .W_z=0,
        .A_x=0,									//惯导加速度
        .A_z=0,
};
#ifdef GYRO_GI410
/*****************************************************/
//GI410陀螺仪


/*****************************************************/
short Last_Azimuth = 0;						//偏航角上次值
short Delta_Azimuth = 0;					//偏航角变化值
uint8_t Flag_Azimuth = 0;					//偏航角上次值接收标志位
//int fputc(int ch, FILE *f)
//{
//uint8_t temp[1] = {ch};
//HAL_UART_Transmit(&GYRO_UART, temp, 1, 0xffff);
//return ch;
//}




static void Gyro_Start(void)
{
    uint8_t cmd_package[9] = {0x5a,0xa5,0x47,0x01,0x01,0x01,0x00,0x4a,0x55};//启动指令
//	temp[0]=0x5a;//帧头
//	temp[1]=0xa5;//帧头
//	temp[2]=0x47;//启动指令
//	temp[3]=1;//启动 0：停止，1：启动
//	temp[4]=1;//判断经纬度
//	temp[5]=1;//判断方位角是否有效
//	temp[6]=0;//预留
//	temp[7]=0x4a;//校验和
//	temp[8]=0x55;//帧尾
    HAL_UART_Transmit(&GYRO_UART,cmd_package,9,0xffff);
}
void Gyro_Reset(void)
{
    uint8_t cmd_package[9] = {0x5a,0xa5,0x48,0x00,0x00,0x00,0x00,0x48,0x55};//启动指令
//	bytes[0]=0x5a;//帧头
//	bytes[1]=0xa5;//帧头
//	bytes[2]=0x48;//输入方位角
//	bytes[3]=0x00;//方位角低字节
//	bytes[4]=0x00;//方位角次低字节
//	bytes[5]=0x00;//方位角次高字节
//	bytes[6]=0x00;//方位角高字节
//	bytes[7]=0x48;//校验和
//	bytes[8]=0x55;//帧尾
    HAL_UART_Transmit(&GYRO_UART,cmd_package,9,0xffff);
}

void Gyro_Init(void)
{
    Gyro_Start();
}

void cal_GI410()
{

    static uint8_t flag = 1;
    static short Angle_pitch,Angle_roll,Angle_azimuth,W_x,W_y,W_z,A_x,A_y,A_z;

    if(gyro.Gyro_Data[0]==0x5A&&gyro.Gyro_Data[1]==0xA5&&gyro.Gyro_Data[47]==0x55)
    {

        gyro.State   = gyro.Gyro_Data[2];
        Angle_pitch   = (short )( (short)gyro.Gyro_Data[4]<<8|gyro.Gyro_Data[3])/10.f;
        Angle_roll    = (short )( (short)gyro.Gyro_Data[6]<<8|gyro.Gyro_Data[5])/10.f;
        Angle_azimuth = (short )( (short)gyro.Gyro_Data[8]<<8|gyro.Gyro_Data[7])/10.f;


        gyro.Angle_pitch    = ((float)Angle_pitch  ) / 10.f;
        gyro.Angle_roll     = ((float)Angle_roll   ) / 10.f;
        gyro.Angle_azimuth  = ((float)Angle_azimuth) / 10.f;

        W_x      = (short )((short)gyro.Gyro_Data[10]<<8|gyro.Gyro_Data[9])/10.f;
        W_y      = (short )((short)gyro.Gyro_Data[12]<<8|gyro.Gyro_Data[11])/10.f;
        W_z      = (short )((short)gyro.Gyro_Data[14]<<8|gyro.Gyro_Data[13])/10.f;

        gyro.W_x = ((float)W_x) / 10.f;
        gyro.W_y = ((float)W_y) / 10.f;
        gyro.W_z = ((float)W_z) / 10.f;

        A_x      = (short )((short)gyro.Gyro_Data[16]<<8|gyro.Gyro_Data[15])/10.f;
        A_y      = (short )((short)gyro.Gyro_Data[18]<<8|gyro.Gyro_Data[17])/10.f;
        A_z      = (short )((short)gyro.Gyro_Data[20]<<8|gyro.Gyro_Data[19])/10.f;
        gyro.A_x = ((float)A_x) / 10.f;
        gyro.A_y = ((float)A_y) / 10.f;
        gyro.A_z = ((float)A_z) / 10.f;
        if(Flag_Azimuth == 4)
        {
            Last_Azimuth = gyro.Angle_azimuth;
        }
        Delta_Azimuth = gyro.Angle_azimuth - Last_Azimuth;
//			printf("变化:%d\r\n",Delta_Azimuth);
        if(gyro.State == 0x02 && flag == 1)
        {
            flag = 0;
            Gyro_Reset();
            HAL_GPIO_WritePin(GPIOA,GPIO_PIN_8,GPIO_PIN_RESET);
        }
#if Printf_Gyro == 1
			printf("pitch:%f roll:%f yaw:%f \r\n",(double)gyro.Angle_pitch/100.f,(double)gyro.Angle_roll/100.f,(double)gyro.Angle_azimuth/100.f);
#endif

    }


}
//void get_GI410()
//{
//	static short Count = 0;
//	static uint8_t lastrxdata;
//	gyro.Gyro_Data[Count] = rxgi410;
//	if(lastrxdata == 0x55 && rxgi410 == 0x5A || Count>0)
//	{
//		Count++;
//	}
//	if(Count == 47)
//	{
//		Count = 0;
//		if(gyro.Gyro_Data[47-1] == 0x55)
//		{
//			cal_GI410(gyro.Gyro_Data);
//		}
//	}
//	lastrxdata = rxgi410;
////	printf("%c",rxgi410);
//}
#endif

#ifdef GYRO_MINS500
extern u_int8_t gyro_sum = 0;
void cal_BW_MINS500() {
static float Last_Yaw = 0;
    if (gyro.Gyro_Data[0] == 0x77 && gyro_sum == gyro.Gyro_Data[13]) {
        gyro.Angle_pitch = (float) ((gyro.Gyro_Data[4] & 0x0f) * 100 + (gyro.Gyro_Data[5] >> 4) * 10 +
                                    (gyro.Gyro_Data[5] & 0x0f) + (gyro.Gyro_Data[6] >> 4) * 0.1f +
                                    (gyro.Gyro_Data[6] & 0x0f) * 0.01f);
        gyro.Angle_roll = (float) ((gyro.Gyro_Data[7] & 0x0f) * 100 + (gyro.Gyro_Data[8] >> 4) * 10 +
                                   (gyro.Gyro_Data[8] & 0x0f) + (gyro.Gyro_Data[9] >> 4) * 0.1f +
                                   (gyro.Gyro_Data[9] & 0x0f) * 0.01f);
        gyro.Angle_azimuth = (float) ((gyro.Gyro_Data[10] & 0x0f) * 100 + (gyro.Gyro_Data[11] >> 4) * 10 +
                                      (gyro.Gyro_Data[11] & 0x0f) + (gyro.Gyro_Data[12] >> 4) * 0.1f +
                                      (gyro.Gyro_Data[12] & 0x0f) * 0.01f);
        if (gyro.Gyro_Data[4] >> 4 == 0x01) {
            gyro.Angle_pitch = -gyro.Angle_pitch;
        }
        if (gyro.Gyro_Data[7] >> 4 == 0x01)
            gyro.Angle_roll = -gyro.Angle_roll;
        if (gyro.Gyro_Data[10] >> 4 == 0x01)
            gyro.Angle_azimuth = -gyro.Angle_azimuth;

        if (gyro.Angle_azimuth <= 360.f && gyro.Angle_azimuth >= 180.f) {
            gyro.Angle_azimuth = gyro.Angle_azimuth - 360.f;
//            if(gyro.Angle_azimuth>180.f)
//                gyro.Angle_azimuth=180.f;
//            if(gyro.Angle_azimuth<-180.f)
//                gyro.Angle_azimuth=-180.f;
        }
//        if(fabsf(gyro.Angle_azimuth-Last_Yaw)>50.f)
//            gyro.Angle_azimuth = Last_Yaw;
        Last_Yaw = gyro.Angle_azimuth;
#if Printf_Gyro == 1
        printf("pitch:%f\troll:%f\tyaw:%f\n",gyro.Angle_pitch,gyro.Angle_roll,gyro.Angle_azimuth);
#endif
    }
}
void GYRO_MINS500_Init()
{
    static uint8_t gyro_rv = 0;
    HAL_UART_Receive_IT(&GYRO_UART,&gyro_rv,1);
}
//接收方式一:串口空闲中断+DMA  放在stm32f4xx_it.c里面
//void USART2_IRQHandler(void)
//{
//    /* USER CODE BEGIN USART2_IRQn 0 */
//    uint32_t temp;
//if((__HAL_UART_GET_FLAG(&huart2,UART_FLAG_IDLE) != RESET))
//    {
//        __HAL_UART_CLEAR_IDLEFLAG(&huart2);
//        HAL_UART_DMAStop(&huart2);
//        temp  =  __HAL_DMA_GET_COUNTER(&hdma_usart2_rx);
////        usb_printf("%d\n",recv_end_flag);
//        rx_len =  BUFFER_SIZE - temp;
//        recv_end_flag = 1;
//    }
//    /* USER CODE END USART2_IRQn 0 */
//    HAL_UART_IRQHandler(&huart2);
//    /* USER CODE BEGIN USART2_IRQn 1 */
//    if(recv_end_flag == 1)
//    {
//        rx_len = 0;
//        recv_end_flag=0;
//        HAL_UART_Receive_DMA(&GYRO_UART,gyro.Gyro_Data,BUFFER_SIZE);
//
//    }
//    /* USER CODE END USART2_IRQn 1 */
//}
//接收方式二，串口回调函数
//void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
//{
//    if(huart->Instance == USART2)
//    {
//        u_int8_t gyro_sum = 0;
//        /******************USER**********************/
//        //数据包长度
//        static uint8_t Rx_len = 14;
//        //包头
//        static uint8_t Head = 0x77;
//        /********************************************/
//        static uint8_t temp = 0;
//        static bool flag_cal_sum = false;
//        static uint8_t Num =0;
//        if(temp == Head && flag_cal_sum == false)
//        {
//            flag_cal_sum = true;
//            Num = 0;
//        }
//        if(flag_cal_sum == true)
//        {
//            if(Num>0 && Num<Rx_len-1)
//                gyro_sum += temp;
//            gyro.Gyro_Data[Num] = temp;
//            Num++;
//            if(Num>=Rx_len-1) {
//                flag_cal_sum = false;
//                gyro_sum = 0;
//            }
//        }
//        HAL_UART_Receive_IT(&GYRO_UART,&temp,1);
//    }
//}
#endif



#ifdef GYRO_100A
uint8_t gyro_receive;
_gyra_100A gyro_100A = {0};


void GYRO_A100_Init(void) {
    HAL_UART_Receive_IT(&GYRO_UART, &gyro_receive, 1);
}

static bool checkGyroDate(void) {
    return (gyro_100A.gyro_buffer[1] ^
            gyro_100A.gyro_buffer[2] ^
            gyro_100A.gyro_buffer[3] ^
            gyro_100A.gyro_buffer[4] == gyro_100A.gyro_buffer[5]);
}

static void getGyroYaw(void) {
    if ((gyro_100A.gyro_buffer[1] & 0x40U) == 0x00U) {
        gyro_100A.gyro_original_angle = (gyro_100A.gyro_buffer[4]) |
                                        (gyro_100A.gyro_buffer[3] << 7) |
                                        (gyro_100A.gyro_buffer[2] << 14) |
                                        (gyro_100A.gyro_buffer[1] << 21);

        gyro_100A.gyro_original_temperature = (gyro_100A.gyro_buffer[7]) |
                                              (gyro_100A.gyro_buffer[6] << 7);
    } else {
        gyro_100A.gyro_original_angle = (gyro_100A.gyro_buffer[4]) |
                                        (gyro_100A.gyro_buffer[3] << 7) |
                                        (gyro_100A.gyro_buffer[2] << 14) |
                                        (gyro_100A.gyro_buffer[1] << 21) |
                                        0xf0000000U;

        gyro_100A.gyro_original_temperature = (gyro_100A.gyro_buffer[7]) |
                                              (gyro_100A.gyro_buffer[6] << 7) |
                                              0xffffc0000U;
    }

    gyro_100A.gyro_temperature = gyro_100A.gyro_original_temperature * GYRO_TEMPERATURE_COEFFICIENT;

    gyro_100A.gyro_angle_s = gyro_100A.gyro_original_angle / GYRO_FACTOR;
    gyro_100A.gyro_angle = gyro_100A.gyro_angle + (gyro_100A.gyro_angle_s * GYRO_TIME) - GYRO_EARTH_SPEED;
    gyro_100A.gyro_angleDate = gyro_100A.gyro_angle /*- gyro_100A.compensation*/;
    gyro.Angle_azimuth = gyro_100A.gyro_angleDate;
#if Printf_Gyro == 1
    printf("gyro_yaw:%f\r\n",gyro.Angle_azimuth);
#endif
    //    return gyro_100A.gyro_angleDate;
}

#if GYRO_RECIVE == 1
void get_gyroValue(void) {

    if (gyro_receive == 0x80) {
        gyro_100A.gyro_buffer[0] = gyro_receive;
        gyro_100A.gyro_location = 1;
    } else {
        if (gyro_100A.gyro_location > 0 && gyro_100A.gyro_location < 8) {
            gyro_100A.gyro_buffer[gyro_100A.gyro_location++] = gyro_receive;
        }
        if (gyro_100A.gyro_location > 7) {
            if (checkGyroDate()) {
                getGyroYaw();
                gyro_100A.gyro_location = 0;
            } else {
                //                gyro_100a.gyro_error_num++;
                gyro_100A.gyro_location = 0;
            }
        }
    }
    //    static uint8_t N = 0;
    //    static uint8_t Gyro_receive = 0;
    //
    //    gyro_100A.gyro_buffer[N] = Gyro_receive;
    //    if(N == 7 && checkGyroDate())
    //    {
    //        if(Gyro_receive == 0x80 )
    //        {
    //            N = 0;
    //        }
    //    }
//     printf("%x\r\n",gyro_100A.gyro_buffer[0]);
    //    N++;
    HAL_UART_Receive_IT(&GYRO_UART, &gyro_receive, 1);
}
#endif
#if GYRO_RECIVE == 8

void get_gyroValue(void)
{

    if(gyro_receive[0] == 0x80)
    {
        for (int i = 0; i < 7; ++i) {
            gyro_100A.gyro_buffer[i] = gyro_receive[i];
        }
        if (checkGyroDate())
        {
            getGyroYaw();
        }
        else
        {
//            gyro_100a.gyro_error_num++;
        }
    }

    HAL_UART_Receive_IT(&huart2, &gyro_receive[8],8);
}

#endif

static float operation_yaw(float Iint_yaw) {
    static float yaw = 0, last_yaw = 0, delta_yaw = 0, final_yaw = 0;
    yaw = Iint_yaw;
    delta_yaw = yaw - last_yaw;
    final_yaw += delta_yaw;
    last_yaw = final_yaw;
    return final_yaw;
}

float returnyaw(void) {
    return (operation_yaw(gyro_100A.gyro_angleDate));
}

#endif